#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>

#include "yuv2rgb.h"
#include "global.h"

#define limit(x) ( (x > 255) ? 255 : ( (x < 0) ? 0 : x ) )

#define CLIP        320
#define RED_NULL    128
#define BLUE_NULL   128
#define LUN_MUL     256
#define RED_MUL     512
#define BLUE_MUL    512

#define GREEN1_MUL  (-RED_MUL/2)
#define GREEN2_MUL  (-BLUE_MUL/6)
#define RED_ADD     (-RED_NULL  * RED_MUL)
#define BLUE_ADD    (-BLUE_NULL * BLUE_MUL)
#define GREEN1_ADD  (-RED_ADD/2)
#define GREEN2_ADD  (-BLUE_ADD/6)

/* lookup tables */
static unsigned int  ng_yuv_gray[256];
static unsigned int  ng_yuv_red[256];
static unsigned int  ng_yuv_blue[256];
static unsigned int  ng_yuv_g1[256];
static unsigned int  ng_yuv_g2[256];
static unsigned int  ng_clip[256 + 2 * CLIP];

#define GRAY(val)               ng_yuv_gray[val]
#define RED(gray,red)           ng_clip[ CLIP + gray + ng_yuv_red[red] ]
#define GREEN(gray,red,blue)    ng_clip[ CLIP + gray + ng_yuv_g1[red] + \
                                                      ng_yuv_g2[blue] ]
#define BLUE(gray,blue)         ng_clip[ CLIP + gray + ng_yuv_blue[blue] ]


void 
yuv2rgb_init(void)
{

	int i;
  
	/* init Lookup tables */
	for (i = 0; i < 256; i++) {
		ng_yuv_gray[i] = i * LUN_MUL >> 8;
		ng_yuv_red[i]  = (RED_ADD    + i * RED_MUL)    >> 8;
		ng_yuv_blue[i] = (BLUE_ADD   + i * BLUE_MUL)   >> 8;
		ng_yuv_g1[i]   = (GREEN1_ADD + i * GREEN1_MUL) >> 8;
		ng_yuv_g2[i]   = (GREEN2_ADD + i * GREEN2_MUL) >> 8;
  	}
  	for (i = 0; i < CLIP; i++)
    		ng_clip[i] = 0;
  	for (; i < CLIP + 256; i++)
    		ng_clip[i] = i - CLIP;
  	for (; i < 2 * CLIP + 256; i++)
    		ng_clip[i] = 255;
}

void   
yuv2rgb (unsigned char* in_addr, unsigned char* out_addr)
{
	unsigned char *y,*u,*v;
	unsigned char *us,*vs;
	unsigned char *dp, *d;
	int i,j,gray;
  
  	dp = (unsigned char *)out_addr;
 	y  = (unsigned char *)in_addr;
	u  = y + width * height;
	v  = u + width * height / 4;
	for (i = 0; i < height; i++) {
		d = dp;
		us = u; vs = v;
		for (j = 0; j < width; j+=2) {
      		gray   = GRAY(*y);
      		*(d++) = RED(gray,*v);
			*(d++) = GREEN(gray,*v,*u);
			*(d++) = BLUE(gray,*u);

			y++;
			gray   = GRAY(*y);
			*(d++) = RED(gray,*v);
			*(d++) = GREEN(gray,*v,*u);
			*(d++) = BLUE(gray,*u);

			y++; u++; v++;
		}
		if (0 == (i % 2)) {
			u = us; v = vs;
		}
		dp += (width*3);
	}

}

